#include "fiveaxiskinematic.h"
#include <math.h>

FiveAxisKinematic::FiveAxisKinematic()
{

}



//#define d2r(d) ((d)*M_PI/180.0)
//#define r2d(r) ((r)*180.0/M_PI)



//static double s2r(double r, double t, double p) {
//    double c;
//    t = d2r(t), p = d2r(p);

//    c.x = r * sin(p) * cos(t);
//    c.y = r * sin(p) * sin(t);
//    c.z = r * cos(p);

//    return c;
//}

//int kinematicsForward(const double *joints,
//              EmcPose * pos,
//              const KINEMATICS_FORWARD_FLAGS * fflags,
//              KINEMATICS_INVERSE_FLAGS * iflags)
//{
//    PmCartesian r = s2r(*(haldata->pivot_length) + joints[8], joints[5], 180.0 - joints[4]);

//    pos->tran.x = joints[0] + r.x;
//    pos->tran.y = joints[1] + r.y;
//    pos->tran.z = joints[2] + *(haldata->pivot_length) + r.z;
//    pos->a = joints[3];
//    pos->b = joints[4];
//    pos->c = joints[5];
//    pos->u = joints[6];
//    pos->v = joints[7];
//    pos->w = joints[8];

//    return 0;
//}

//int kinematicsInverse(const EmcPose * pos,
//              double *joints,
//              const KINEMATICS_INVERSE_FLAGS * iflags,
//              KINEMATICS_FORWARD_FLAGS * fflags)
//{

//    PmCartesian r = s2r(*(haldata->pivot_length) + pos->w, pos->c, 180.0 - pos->b);

//    joints[0] = pos->tran.x - r.x;
//    joints[1] = pos->tran.y - r.y;
//    joints[2] = pos->tran.z - *(haldata->pivot_length) - r.z;
//    joints[3] = pos->a;
//    joints[4] = pos->b;
//    joints[5] = pos->c;
//    joints[6] = pos->u;
//    joints[7] = pos->v;
//    joints[8] = pos->w;

//    return 0;
//}

int FiveAxisKinematic::kinematicsInverse(std::vector<double> posIn, std::vector<double> &jointOut)
{
    jointOut=posIn;
    jointOut.resize(5);

    return 1;
}

int FiveAxisKinematic::kinematicsForward(std::vector<double> jointIn, std::vector<double> &posOut)
{
    posOut=jointIn;
    posOut.resize(6);
    posOut[5]=0;
    return 1;
}

